let ros = null
let commandTopic = null

function sendCommand(cmd_id, params = {}) {
  if (!commandTopic) return
  const now = Date.now() * 1000
  const msg = {
    timestamp: now,
    command: cmd_id,
    target_system: 1,
    target_component: 1,
    source_system: 1,
    source_component: 1,
    confirmation: 0,
    from_external: true,
    param1: params.param1 || 0,
    param2: params.param2 || 0,
    param3: params.param3 || 0,
    param4: params.param4 || 0,
    param5: params.param5 || 0,
    param6: params.param6 || 0,
    param7: params.param7 || 0
  }
  commandTopic.publish(msg)
  console.log('[COMMAND]', msg)
}

export default {
  init(rosInstance) {
    ros = rosInstance
    commandTopic = new ROSLIB.Topic({
      ros: ros,
      name: '/fmu/in/vehicle_command',
      messageType: 'px4_msgs/msg/VehicleCommand'
    })
  },
  sendFlightMode(modeInt = 6) {
    sendCommand(176, { param1: 1, param2: modeInt }) // MAV_CMD_DO_SET_MODE
  },
  arm() {
    sendCommand(400, { param1: 1 }) // ARM
  },
  disarm() {
    sendCommand(400, { param1: 0 }) // DISARM
  },
  takeoff(alt = 10) {
    sendCommand(22, { param7: alt }) // TAKEOFF
  },
  land() {
    sendCommand(21) // LAND
  },
  goto(lat, lon, alt) {
    sendCommand(16, { param5: lat, param6: lon, param7: alt }) // WAYPOINT
  }
}
